#include "protocal.h"
#include "protocal_define.h"
#include "respond_struct_define.h"
#include "socket.h"
#include <arpa/inet.h>
#include <ifaddrs.h>
#include <plog/Log.h>
#include <thread>

#define MAX_IQ_PACKET_COUNT 256

Protocal::Protocal(uint8_t device_type, Radar *radar, SystemParameters *paramteres)
    : _device_type(device_type), m_radar(radar), system_parameters(paramteres) {
    _last_ip_octet = Socket::getLocalIPLastOctet("eth0");
}

Protocal::~Protocal(void) {}

void Protocal::SendResponse(uint8_t cmd) {
    rspuniversal_t rsp;
    rsp.head = HEAD;
    rsp.tail = TAIL;
    rsp.checksum = 0x00;
    rsp.cmd = cmd;
    rsp.cmdrespond = 0; // 执行成功
    int acturallenght = _send_callback((const uint8_t *)&rsp, sizeof(rspuniversal_t));
}

/*
function: 处理来自服务端的数据
param:  buffer 数据指针
        len 数据长度
*/
void Protocal::process_msg(const uint8_t *buffer, int len) {
    PLOG_DEBUG << "Received data length: " << len;
    for (int i = 0; i < len; i++) {
        printf("0x%02x ", buffer[i]);
    }
    if (len <= 0) {
        PLOG_ERROR << "Incorrect data length!";
        return;
    }
    /*if(buffer[0] != (uint8_t)0x90 && buffer[1] != (uint8_t)0xea) {
        PLOG_ERROR << "data head error";
        return;
    }*/

    std::cout << std::endl;
    switch ((uint8_t)buffer[2]) {
    case CMDHEARTBEAT:
        PLOG_DEBUG << "receive heartbeat cmd";
        ProcessHeartbeat();
        break;
    case CMDGETSIGNALTABLE:
        PLOG_DEBUG << "receive get signal table cmd";
        ProcessGetSignalTable(buffer, len);
        break;
    case CMDSETPAYLOADPARAMETER:
        PLOG_DEBUG << "receive set payloda parameter cmd";
        ProcessSetPayloadParameter(buffer, len);
        break;
    case CMDUAVCTRL:
        PLOG_DEBUG << "receive uav ctrl cmd";
        ProcessUavFlightCtrl(buffer, len);
        break;
    case CMDSETREMOTEPOWEROFF:
        PLOG_DEBUG << "receive set remote power off cmd";
        ProcessRemotePoweroff(buffer, len);
        break;
    case CMDSETFLIGHTCTRLPARAMETER:
        PLOG_DEBUG << "receive set flight ctrl parameter cmd";
        ProcessFlightCtrlParameter(buffer, len);
        break;
    case CMDSETPAYLOADCONTROL:
        PLOG_DEBUG << "receive ser payload ctrl cmd";
        ProcessPayloadCtrl(buffer, len);
        break;
    case CMDGETUAVSTATUS:
        PLOG_DEBUG << "receive get uav status cmd";
        ProcessGetUAVStatus(buffer, len);
        break;
    case CMDGETPAYLOADSTATUS:
        PLOG_DEBUG << "receive get payload status cmd";
        ProcessGetPayloadStatus(buffer, len);
        break;
    case CMDIQINFORMATION:
        PLOG_DEBUG << "receive position mid frequence cmd";
        ProcessPositionMidFreq(buffer, len);
        break;
    case CMDDATAREMOVE:
        PLOG_DEBUG << "receive data remove cmd";
        ProcessDataRemove(buffer, len);
        break;
    case CMDTARGETPOSITON:
        PLOG_DEBUG << "receive target position cmd";
        ProcessTargetPosition(buffer, len);
    case CMDTARGETLOADSIMULATEDATA:
        PLOG_DEBUG << "receive target load simulate data";
        ProcessTargetLoadSimulateData(buffer, len);
        break;
    default:
        PLOG_DEBUG << "receive cmd:" << (int)buffer[3];
        break;
    }
}

/*
function: 处理心跳指令并返回数据
*/
void Protocal::ProcessHeartbeat(void) {
    rspheartbeat_t rsp;
    rsp.head = HEAD;
    rsp.tail = TAIL;
    rsp.checksum = 0x0000;
    rsp.cmd = 0x00;
    int acturallenght = _send_callback((const uint8_t *)&rsp, sizeof(rspheartbeat_t));
    PLOG_DEBUG << "send actural length: " << acturallenght;
}

void Protocal::sendSignalTable() {
    rspgetsignaltable1_t rsp;
    std::unique_lock<std::mutex> lck(*(m_radar->returnMutex()));
    std::list<signaltable_t> *signal_found = m_radar->returnSignalFound();

    rsp.head = HEAD;
    rsp.cmd = CMDGETSIGNALTABLE;

    if (signal_found != nullptr)
        rsp.N = signal_found->size();
    else
        rsp.N = 0;

    rsp.N = rsp.N > 50 ? 50 : rsp.N; // 最多只允许50个信号通过，否则传不完

    uint16_t full_signal_length = sizeof(signaltable_t);
    uint16_t actual_rsp_size = sizeof(rspgetsignaltable1_t) + rsp.N * full_signal_length;
    uint8_t actual_rsp[actual_rsp_size];
    rsp.tail = TAIL;
    rsp.checksum = 0;
    uint32_t offset = 0;
    PLOG_DEBUG << "信号表数量：" << rsp.N;
    memcpy((void *)&actual_rsp[offset], (const void *)&rsp, 5); // 拷贝包头，cmd和信号数量
    offset += 5;

    std::list<signaltable_t>::iterator it = signal_found->begin();
    for (int i = 0; i < rsp.N; i++) {
        signaltable_t &psignal = *it;
        memcpy((void *)&actual_rsp[offset], (const void *)&psignal, full_signal_length);
        offset += full_signal_length;
        it++;
    }
    memcpy((void *)&actual_rsp[offset], (const void *)&rsp.checksum, 4); // 拷贝校验和和包尾
    int acturallenght = _send_callback((const uint8_t *)&actual_rsp, actual_rsp_size);
    PLOG_DEBUG << "GetSignalTable send actural length: " << acturallenght;
    PLOG_DEBUG << "start transmit signal table.";
}

/*
function: 处理获取信号表命令
*/
void Protocal::ProcessGetSignalTable(const uint8_t *buffer, int len) {
    getsingaltablecmd_t *cmd;
    cmd = (getsingaltablecmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }
    if (cmd->receivectrl == 1) {
        sendSignalTable();
    } else {
        SendResponse(CMDGETSIGNALTABLE);
        PLOG_DEBUG << "GetSignalTable send";
        PLOG_DEBUG << "stop transmit signal table.";
    }
}

/*
function: 处理设置载荷参数命令, 不定长接收
靶标参数设置发送报文
*/
void Protocal::ProcessSetPayloadParameter(const uint8_t *buffer, int len) {
    // 靶机
    PLOG_INFO << "device type code: " << (int)_device_type;
    uint16_t offset_p = 0;
    if (_device_type == 0) {
        setpayloadparametercmdhead_t *head;
        head = (setpayloadparametercmdhead_t *)buffer;
        if (head->head != HEAD) {
            PLOG_ERROR << "cmd head error";
            return;
        }
        PLOG_INFO << "辐射信号数: " << (int)head->signalNum;
        offset_p += 4;
        target_signal_def *signal = (target_signal_def *)&buffer[offset_p];
        offset_p += sizeof(target_signal_def);
        PLOG_INFO << "信号序号：" << (int)1;
        PLOG_INFO << "辐射功率: " << (int)signal->power;
        PLOG_INFO << "信号类型: " << (int)signal->type;
        PLOG_INFO << "辐射频率: " << signal->frequency;
        PLOG_INFO << "辐射带宽: " << signal->bandwidth;
        PLOG_INFO << "调制方式: " << (int)signal->modulation;
        PLOG_INFO << "帧延时: " << signal->frameDelay;
        PLOG_INFO << "信号帧结构: " << (int)signal->frameStructure;
        m_radar->emit_signal(signal);
        system_parameters->set_target_signal(signal);

        setpayloadparametercmdtail_t *tail;
        tail = (setpayloadparametercmdtail_t *)&buffer[offset_p];
        // tail->checksum;
        if (tail->tail != TAIL) {
            PLOG_ERROR << "cmd tail error";
            return;
        }
    } else if (_device_type == 1) {
        sense_config_para_t *head;
        head = (sense_config_para_t *)buffer;
        if (head->head != HEAD) {
            PLOG_ERROR << "cmd head error";
            return;
        }
        PLOG_INFO << "频谱感知模式: " << (int)head->mode;
        PLOG_INFO << "感知点频率上限: " << (int)head->maxFrequency;
        PLOG_INFO << "感知点频率下限: " << (int)head->minFrequency;
        PLOG_INFO << "感知点带宽上限: " << (int)head->maxBandwidth;
        PLOG_INFO << "感知点带宽下限: " << (int)head->minBandwidth;
        PLOG_INFO << "感知点驻留时间: " << (float)head->residenceTime;
        PLOG_INFO << "感知信号数: " << (int)head->signalNum;
        setairpayloadparametercmdtail_t *tail =
            (setairpayloadparametercmdtail_t
                 *)&buffer[offset_p + sizeof(sense_config_para_t) +
                           head->signalNum * sizeof(setairpayloadsignaldescrip_t)];
        // tail->checksum;
        if (tail->tail != TAIL) {
            PLOG_ERROR << "cmd tail error";
            return;
        }

        m_radar->set_sense_config_parameters(head); // 设置载荷参数
        system_parameters->set_sense_config_parameters(head);
        offset_p += sizeof(sense_config_para_t);

        setairpayloadsignaldescrip_t *signal;
        m_radar->clearSignalUnderDetection();

        for (uint8_t i = 0; i < head->signalNum; i++) {
            signal = (setairpayloadsignaldescrip_t *)&buffer[offset_p];
            PLOG_INFO << "信号序号：" << (int)i + 1;
            PLOG_INFO << "窄带扫描频率: " << (int)signal->frequency;
            PLOG_INFO << "窄带扫描带宽: " << (int)signal->bandwidth;
            offset_p += sizeof(setairpayloadsignaldescrip_t);

            m_radar->addSignalUnderDetection(i, signal->frequency, signal->bandwidth,
                                             head->attenuation);
            system_parameters->set_narrowband_freq(signal->frequency, signal->bandwidth,
                                                   head->attenuation);

            // 暂时只添加一个信号，目前系统只支持一个信号，切换信号由上位机统一控制
            break;
        }
    } else {
        PLOG_INFO << "device type code: " << (int)_device_type << "nothing to do.";
        return;
    }

    SendResponse(CMDSETPAYLOADPARAMETER);
    PLOG_DEBUG << "SetPayloadParameter send";
}

/*
function: 处理飞控指令
*/
void Protocal::ProcessUavFlightCtrl(const uint8_t *buffer, int len) {
    uavflightctrlcmd_t *cmd;
    cmd = (uavflightctrlcmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }
    // TODO: 添加与硬件交互的功能
    uint8_t execreslut = 0; // 默认执行成功
    switch (cmd->control) {
    case UAVNORESPOND:
        break;
    case UAVTAKEOFF:
        break;
    case UAVHOVER:
        break;
    case UAVFLIGHT:
        break;
    case UAVLAND:
        break;
    }

    SendResponse(CMDUAVCTRL);
    PLOG_DEBUG << "UavFlightCtrl send";
}

/*
function: 处理远程开关机指令
*/
void Protocal::ProcessRemotePoweroff(const uint8_t *buffer, int len) {
    setremotepoweroffcmd_t *cmd;
    cmd = (setremotepoweroffcmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }
    uint8_t execreslut = 0;         // 默认执行成功
    if (cmd->control & 0x01 == 1) { // 关机
        // TODO: 添加与硬件交互功能
        PLOG_INFO << "收到关机指令";
    } else { // 开机
        // TODO: 添加与硬件交互功能
        PLOG_INFO << "收到开机指令";
    }
    SendResponse(CMDSETREMOTEPOWEROFF);
    PLOG_DEBUG << "RemotePoweroff send";
}

/*
function: 处理飞控参数设置指令
*/
void Protocal::ProcessFlightCtrlParameter(const uint8_t *buffer, int len) {
    setflightctrlparametercmd_t *cmd;
    cmd = (setflightctrlparametercmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }

    SendResponse(CMDSETFLIGHTCTRLPARAMETER);
    PLOG_DEBUG << "RemotePoweroff send";
}

/*
function: 处理载荷控制
*/
void Protocal::ProcessPayloadCtrl(const uint8_t *buffer, int len) {
    setpayloadctrlcmd_t *cmd;
    cmd = (setpayloadctrlcmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }
    /*0-开启载荷功能，1-关闭载荷功能，2-节点远程重启，3-节点远程关机
     */
    switch (cmd->enable) {
    case 0: {
        PLOG_INFO << "开启载荷功能";
        // 绑定两个回调函数，暂时没有更好的地方执行这个操作
        m_radar->setSendIQCallback(
            std::bind(&Protocal::SendIQ2Computer, this, std::placeholders::_1));
        m_radar->setSendSignalTableCallback(std::bind(&Protocal::sendSignalTable, this));
        m_radar->notifySense();
        break;
    }
    case 1:
        PLOG_INFO << "关闭载荷功能";
        m_radar->stopNarrowBandScan();
        break;
    case 2:
        PLOG_INFO << "节点远程重启";
        break;
    case 3:
        PLOG_INFO << "节点远程关机";
        break;
    default:
        break;
    }

    SendResponse(CMDSETPAYLOADCONTROL);
    PLOG_DEBUG << "PayloadCtrlParameter send";
}

/*
function: 处理无人机状态获取指令
*/
void Protocal::ProcessGetUAVStatus(const uint8_t *buffer, int len) {
    getuavstatuscmd_t *cmd;
    cmd = (getuavstatuscmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }

    /*
    飞机状态读取1
    b0:读取飞行状态，0-否，1-是
    b1:读取飞机电池信息，0-否，1-是
    b2:读取飞机经度纬度、海拔高度、对地高度，0-否，1-是
    b3:读取飞机速度、油门，0-否，1-是
    b4:读取飞机姿态、航向，0-否，1-是
    b5:读取飞机当前航点，0-否，1-是
    b6:读取飞机遥控器数据，0-否，1-是
    b7:读取飞机健康管理信息，0-否，1-是
    飞机状态读取2
    b0:读取飞机告警信息，0-否，1-是
    b1~b7:预留
    */
    if (cmd->status1 & 0x01) {
    }
    if (cmd->status1 & 0x02) {
    }
    if (cmd->status1 & 0x04) {
    }
    if (cmd->status1 & 0x08) {
    }
    if (cmd->status1 & 0x10) {
    }
    if (cmd->status1 & 0x20) {
    }
    if (cmd->status1 & 0x40) {
    }
    if (cmd->status1 & 0x80) {
    }
    if (cmd->status2 & 0x01) {
    }
    rspuavstatus1_t rsp;
    rsp.head = HEAD;
    rsp.tail = TAIL;
    rsp.cmd = CMDGETUAVSTATUS;
    rsp.checksum = checksum_gen((uint8_t *)&rsp.cmd, sizeof(rspuavstatus1_t) -
                                                         sizeof(rsp.checksum) - sizeof(rsp.tail) -
                                                         sizeof(rsp.reserved) - sizeof(rsp.head));
    rsp.uavstatus = 0x01;
    rsp.uavvoltage = 0x15;
    rsp.uavremainder = 0x32;
    rsp.uavbattery = 0x20;
    rsp.uavlongitude = 0x34;
    rsp.uavlatitude = 0x35;
    rsp.uavheight = 0x36;
    rsp.uavaltitude = 0x37;
    rsp.uavdirection = 0x45;
    rsp.uavspeed = 0x46;
    rsp.uavattitudeangle = 0x47;
    rsp.uavazimuth = 0x48;
    rsp.uavwaypointnumber = 0x02;
    rsp.uavwaypointlongitude = 0x03;
    rsp.uavwaypointlatitude = 0x04;
    rsp.uavwaypointheight = 0x05;
    rsp.uavhealthyinfo = 0x00;
    rsp.uavwarninginfo = 0x0f;
    int acturallenght = _send_callback((const uint8_t *)&rsp,
                                       sizeof(rspuavstatus1_t)); // 不同的端口需要修改不同的端口号
    PLOG_DEBUG << "GetUAVStatus send actural length: " << acturallenght;
}

/*
function: 处理载荷状态获取指令
*/
void Protocal::ProcessGetPayloadStatus(const uint8_t *buffer, int len) {
    getpayloadstatuscmd_t *cmd;
    cmd = (getpayloadstatuscmd_t *)buffer;

    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }

    if (cmd->dataread & 0x01) {
        // 读取载荷状态信息
    }
    if (cmd->dataread & (0x01 << 1)) {
        // 读取载荷参数设置
    }
    if (cmd->dataread & (0x01 << 2)) {
        // 读取载荷告警信息
    }

    const payload_status_t status = system_parameters->get_payload_status();
    int acturallenght = _send_callback((const uint8_t *)&status, sizeof(payload_status_t));
    PLOG_DEBUG << "GetPayloadStatus send actural length: " << acturallenght;
}

/*
function: 处理定位中频数据指令
*/
void Protocal::ProcessPositionMidFreq(const uint8_t *buffer, int len) {
    positionmidfreqcmd_t *cmd;
    cmd = (positionmidfreqcmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }
    if (cmd->midfreqctrl == 1) {
        // 开始任务
        PLOG_DEBUG << "开始任务";
        m_radar->startIQ_TX_Task();
    } else {
        // 结束任务
        PLOG_DEBUG << "结束任务";
        m_radar->stopIQ_TX_Task();
    }
}

/*
function: 处理数据卸载指令
*/
void Protocal::ProcessDataRemove(const uint8_t *buffer, int len) {
    uint32_t offset = 0;
    dataremovecmd_t *cmd;
    cmd = (dataremovecmd_t *)buffer;
    if (cmd->head != HEAD || cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }
    uint8_t cmd1 = (cmd->dataremovecmd & 0x0f);
    uint8_t cmd2 = ((cmd->dataremovecmd & 0xf0) >> 4);
    PLOG_INFO << "数据卸载指令码1: " << (int)cmd1;
    PLOG_INFO << "数据卸载指令码2: " << (int)cmd2;
    PLOG_INFO << "编号: " << (int)cmd->id;
    /*
    b0~b3:0-停止传输数据，1-开始传输数据
    ，2-暂停传输数据，3-继续传输数据，4-获取数据总大小，5-获取传输百分比，其它-备份，默认值=0 b4~b7:
    0-不响应，1-读取信号表 ，2-卸载指定N个信号的中频数据
    */
    uint8_t *actual_rsp;
    uint32_t full_signal_length;
    uint32_t full_rsp_length;
    int acturallenght;
    rspdataremove_t rsp;
    switch (cmd2) {
    case 1: {
        GetSignalTable(cmd);
        break;
    }
    case 2:
        RemoveMidData(cmd);
        break;
    default:
        break;
    }
}

void Protocal::GetSignalTable(dataremovecmd_t *cmd) {
    uint32_t offset = 0;
    uint32_t full_signal_length;
    uint32_t full_rsp_length;
    rspdataremove_t rsp;
    PLOG_INFO << "读取信号表";
    // 获取检测中的信号表
    std::unique_lock<std::mutex> lck(*(m_radar->returnMutex()));
    std::list<signaltable_t> *signal_under_detection = m_radar->returnSignalUnderDetection();

    full_signal_length = sizeof(signaltable_t);
    full_rsp_length = signal_under_detection->size() * full_signal_length + sizeof(rspdataremove_t);
    uint8_t actual_rsp[full_rsp_length];
    rsp.head = HEAD;
    rsp.tail = TAIL;
    rsp.cmd = CMDDATAREMOVE;
    rsp.checksum = 0;
    rsp.datatype = 0;
    rsp.length = signal_under_detection->size();
    rsp.datasize = rsp.length * full_signal_length; // 随便填的
    memcpy((void *)&actual_rsp[offset], (const void *)&rsp, 16);
    offset += 16;

    for (signaltable_t &psignal : *signal_under_detection) {
        memcpy((void *)&actual_rsp[offset], (const void *)&psignal, full_signal_length);
        offset += full_signal_length;
    }
    memcpy((void *)&actual_rsp[offset], (const void *)&rsp.checksum, 4); // 拷贝校验和和包尾
    int acturallenght = _send_callback((const uint8_t *)actual_rsp,
                                       full_rsp_length); // 不同的端口需要修改不同的端口号
    PLOG_DEBUG << "DataRemove send actural length: " << acturallenght;
}

/*
function: 数据卸载指令中的卸载中频数据指令
*/
void Protocal::RemoveMidData(dataremovecmd_t *cmd) {
    rspdataremove_t rsp;

    PLOG_INFO << "卸载中频数据";
    PLOG_INFO << "指定卸载信号的频率: " << (int)cmd->frequency;
    PLOG_INFO << "指定卸载信号的带宽: " << (int)cmd->bandwidth;

    // TODO: 删除指定频率的信号表

    // 获取检测中的信号表
    std::unique_lock<std::mutex> lck(*(m_radar->returnMutex()));
    std::list<signaltable_t> *signal_under_detection = m_radar->returnSignalUnderDetection();
    std::list<signaltable_t>::iterator it;
    for (it = signal_under_detection->begin(); it != signal_under_detection->end(); ++it) {
        if (it->frequency == cmd->frequency && it->bandwidth == cmd->bandwidth) {
            // it = signal_under_detection->erase(it); // 使用erase方法并更新迭代器
            break;
        }
    }

    signaltable_t *signal = &(*it);
    if (it != signal_under_detection->end()) {
        IQstruct_t iq;
        rsp.length = signal->IQ_size;
        uint32_t full_rsp_length = rsp.length * sizeof(IQstruct_t) + sizeof(rspdataremove_t);
        uint8_t actual_rsp[full_rsp_length];

        /* 包头包尾  */
        rsp.head = HEAD;
        rsp.tail = TAIL;
        rsp.cmd = CMDDATAREMOVE;
        rsp.checksum = 0;
        rsp.datatype = 1;
        rsp.rate = 100;
        uint32_t offset = 16; // 前16个字节单独拷贝

        for (uint32_t i = 0; i < rsp.length; i++) {
            memcpy((void *)&actual_rsp[offset], (const void *)&signal->IQ_table[i],
                   sizeof(IQstruct_t));
            offset += sizeof(IQstruct_t);
        }
        rsp.datasize = rsp.length * sizeof(IQstruct_t);
        memcpy((void *)&actual_rsp[0], (const void *)&rsp, 16);
        memcpy((void *)&actual_rsp[offset], (const void *)&rsp.checksum,
               4); // 拷贝校验和和包尾
        int acturallenght = _send_callback((const uint8_t *)actual_rsp,
                                           full_rsp_length); // 不同的端口需要修改不同的端口号
        PLOG_DEBUG << "DataRemove send actural length: " << acturallenght;
    } else {
    }
}

/*
function: 处理靶机定位指令
*/
void Protocal::ProcessTargetPosition(const uint8_t *buffer, int len) {
    positiontargetcmd_t *cmd;
    cmd = (positiontargetcmd_t *)buffer;
    if (cmd->head != HEAD && cmd->tail != TAIL) {
        PLOG_ERROR << "cmd head or tail error";
        return;
    }

    rsptargetposition1_t rsp;
    rsp.head = HEAD;
    rsp.tail = TAIL;
    rsp.cmd = CMDTARGETPOSITON;
    rsp.checksum = 0;
    int acturallenght = _send_callback((const uint8_t *)&rsp, sizeof(rsptargetposition1_t));
    PLOG_DEBUG << "TargetPosition send actural length: " << acturallenght;
}

/*
function: 靶标加载模拟数据指令
*/
void Protocal::ProcessTargetLoadSimulateData(const uint8_t *buffer, int len) {
    targetloadsimulatedatacmdhead_t *head;
    uint16_t offset = 0;
    head = (targetloadsimulatedatacmdhead_t *)buffer;
    if (head->head != HEAD) {
        PLOG_ERROR << "cmd head error";
        return;
    }
    PLOG_INFO << "加载信号序号: " << (int)head->loadsignalnumber;
    PLOG_INFO << "信号子包序号: " << (int)head->subpktnumber;
    PLOG_INFO << "采样率: " << head->samplerate;
    PLOG_INFO << "IQ数据长度: " << (int)head->IQdatalength;
    offset += sizeof(targetloadsimulatedatacmdhead_t);
    for (uint16_t i = 0; i < head->IQdatalength; i++) {
        IQstruct_t *IQ = (IQstruct_t *)&buffer[offset];
        PLOG_INFO << "I: " << IQ->IQI << "Q: " << IQ->IQQ;
        offset += sizeof(IQstruct_t);
    }
    targetloadsimulatedatacmdtail_t *tail = (targetloadsimulatedatacmdtail_t *)&buffer[offset];
    // tail->checksum;
    if (tail->tail != TAIL) {
        PLOG_ERROR << "cmd tail error";
        return;
    }
    // TODO: 添加指令解析代码，添加与硬件交互代码
    rspuniversal_t rsp;
    rsp.head = HEAD;
    rsp.tail = TAIL;
    rsp.cmd = CMDTARGETLOADSIMULATEDATA;
    rsp.checksum = 0;
    int acturallenght = _send_callback((const uint8_t *)&rsp,
                                       sizeof(rspuniversal_t)); // 不同的端口需要修改不同的端口号
    PLOG_DEBUG << "TargetLoadSimulateData send actural length: " << acturallenght;
}

void Protocal::set_send_callback(const std::function<int(const uint8_t *, int)> callback) {
    _send_callback = callback;
    system_parameters->start_update_thread();
}

/* 发送IQ数据到上位机 */
void Protocal::SendIQ2Computer(signaltable_t *signaltable) {
    iq_information_t rsp = {.head = HEAD,
                            .cmd = CMDIQINFORMATION,
                            .signalid = static_cast<uint8_t>(signaltable->id),
                            .sensenodeip = _last_ip_octet,
                            .sampleFrequency = 0,
                            .arriveTime = signaltable->arrivetime,
                            .endtime = signaltable->endtime,
                            .frequency = signaltable->frequency,
                            .bandwidth = signaltable->bandwidth,
                            .latitude = signaltable->latitude,
                            .longitude = signaltable->longitude,
                            .height = signaltable->height,
                            .speed = signaltable->speed,
                            .angle = signaltable->angle,
                            .checksum = 0,
                            .tail = TAIL};
    uint32_t iq_packet_total_size = signaltable->IQ_size;
    const uint32_t MAX_IQ_PACKET_SIZE = MAX_IQ_PACKET_COUNT * sizeof(IQstruct_t);
    rsp.packet_total_num = signaltable->IQ_size / MAX_IQ_PACKET_SIZE +
                           ((signaltable->IQ_size % MAX_IQ_PACKET_SIZE > 0) ? 1 : 0);

    PLOG_INFO << "开始发送IQ数据。。。";
    PLOG_INFO << "IQ数据长度：" << iq_packet_total_size;
    _send_callback((const uint8_t *)&rsp, sizeof(iq_information_t)); // 发送IQ信息

    iq_bulk_t iq_bulk_head = {.head = HEAD, .cmd = CMDIQBULK};
    packet_tail_t iq_bulk_tail = {.tail = TAIL};
    uint8_t iq_bulk_packet[MAX_IQ_PACKET_SIZE + sizeof(iq_bulk_t) + sizeof(packet_tail_t)];
    uint16_t packet_num = 0;

    // IQ数据太长，分包发送，避免到达Protocal包长限制
    while (iq_packet_total_size > MAX_IQ_PACKET_SIZE) {
        iq_bulk_head.bulk_seq = packet_num;
        iq_bulk_head.iq_packet_size = MAX_IQ_PACKET_SIZE;
        // 依次考入包头、IQ数据、包尾
        memcpy((void *)&iq_bulk_packet[0], (const void *)&iq_bulk_head, sizeof(iq_bulk_t));
        memcpy((void *)&iq_bulk_packet[sizeof(iq_bulk_t)],
               (const void *)(signaltable->IQ_table + MAX_IQ_PACKET_COUNT * packet_num),
               MAX_IQ_PACKET_SIZE);
        memcpy((void *)&iq_bulk_packet[sizeof(iq_bulk_t) + MAX_IQ_PACKET_SIZE],
               (const void *)&iq_bulk_tail, sizeof(iq_bulk_tail));
        int acturallenght =
            _send_callback((const uint8_t *)&iq_bulk_packet, sizeof(iq_bulk_packet));

        packet_num += 1;
        iq_packet_total_size -= MAX_IQ_PACKET_SIZE;

        /* 限制 Protocal 发送速率，避免丢包！！！ */
        std::this_thread::sleep_for(std::chrono::microseconds(500));

        // PLOG_DEBUG << "IQ send packet num:" << packet_num << "  actural length: " <<
        // acturallenght;
    }

    // 剩下最后一包，小于最大IQ长度，单独计算发送
    if (iq_packet_total_size > 0) {
        iq_bulk_head.bulk_seq = packet_num;
        iq_bulk_head.iq_packet_size = iq_packet_total_size;
        PLOG_INFO << "发送最后一包IQ数据";
        memcpy((void *)&iq_bulk_packet[0], (const void *)&iq_bulk_head, sizeof(iq_bulk_t));
        memcpy((void *)&iq_bulk_packet[sizeof(iq_bulk_t)],
               (const void *)(signaltable->IQ_table + MAX_IQ_PACKET_COUNT * packet_num),
               iq_packet_total_size);
        memcpy((void *)&iq_bulk_packet[sizeof(iq_bulk_t) + iq_packet_total_size],
               (const void *)&iq_bulk_tail, sizeof(packet_tail_t));

        int acturallenght =
            _send_callback((const uint8_t *)&iq_bulk_packet,
                           iq_packet_total_size + sizeof(iq_bulk_t) + sizeof(packet_tail_t));
        // PLOG_DEBUG << "IQ send packet num:" << packet_num << "  actural length: " <<
        // acturallenght;
    }
}
